#include "decision.h"
#include <yaml-cpp/yaml.h>
#include <nav_msgs/GetPlan.h>
#include <std_msgs/Int8.h>

DecisionCenter::DecisionCenter(ros::NodeHandle& n)
{
    nh = n;
    std::string odom_topic,perception_topic,plan_topic,limit_speed_topic,scan_topic,goal_path;
    nh.getParam("odom_topic",odom_topic);
    nh.getParam("perception_topic",perception_topic); 
    nh.getParam("goal_path",goal_path);
    nh.getParam("plan_topic",plan_topic);
    nh.getParam("scan_topic",scan_topic);
    nh.getParam("limit_speed_topic",limit_speed_topic);
    odom_sub_ = nh.subscribe(odom_topic,5,&DecisionCenter::odom_callback,this);
    perception_sub_ = nh.subscribe(perception_topic,5,&DecisionCenter::perception_callback,this);
    scan_sub_ = nh.subscribe(scan_topic,5,&DecisionCenter::scan_callback,this);
    plan_pub_ = nh.advertise<nav_msgs::Path>(plan_topic,5);
    limit_speed_pub_ = nh.advertise<std_msgs::Int8>(limit_speed_topic,5);
    plan_client_ = nh.serviceClient<nav_msgs::GetPlan>("/move_base/GlobalPlanner/make_plan");
    read_goal(goal_path);
    current_status_ = 0;
    current_goal_ = 0;
    goal_thres = 0.7;
    obstacle_detection_init();
}

void DecisionCenter::read_goal(std::string& goal_path)
{
    YAML::Node config = YAML::LoadFile(goal_path);
    auto goal_list = config["goal_list"].as<std::vector<std::vector<double>>>();
    for(auto goal:goal_list)
    {
        geometry_msgs::Point p;
        p.x = goal[0];
        p.y = goal[1];
        goal_list_.emplace_back(p);
    }
}

void DecisionCenter::odom_callback(const nav_msgs::OdometryConstPtr& msg)
{
    current_pose_ = msg->pose.pose;
    decision_machine();
    if(enable_pass_control_)
    {
        pass_condition(goal_list_[current_goal_],goal_thres);
    }
}

void DecisionCenter::scan_callback(const sensor_msgs::LaserScanConstPtr& msg)
{
    static bool detect_obstacle = false;
    if(!detect_obstacle)
    {
        if(pow(current_pose_.position.x-obstacle_params.start.x,2)+pow(current_pose_.position.y-obstacle_params.start.y,2)<0.5)
        {
            detect_obstacle = true;
        }
    }
    else
    {
        if(pow(current_pose_.position.x-obstacle_params.end.x,2)+pow(current_pose_.position.y-obstacle_params.end.y,2)<0.5)
        {
            detect_obstacle = false;
        }
        else{
            int start_idx = (obstacle_params.min_angle-msg->angle_min)/msg->angle_increment;
            int end_idx = (obstacle_params.max_angle-msg->angle_min)/msg->angle_increment;
            int size = 0;
            for(int i=start_idx;i<end_idx;i++)
            {
                if(msg->ranges[i]<0.6)
                {
                    size++;
                }
            }
            std::cout<<size<<std::endl;
            if(size>5)
                stop();
            else
                dislimit_speed();
            ROS_INFO("Detecting obstacle");
        }
    }
}

void DecisionCenter::perception_callback(const std_msgs::Int8ConstPtr& msg)
{
    if(current_status_ == 0 && msg->data == 0)
        current_status_++;
    else if(msg->data == 1)     //speed limit
        limit_speed();
    else if(msg->data == 2)    //解除限速
        dislimit_speed();
}

void DecisionCenter::decision_machine()
{
    static int last_status_ = -1;
    if(current_status_ != last_status_)
    {
        switch(current_status_)
        {
            case 0:
                wait();
                break;
            case 1:
                goal_control();
                enable_pass_control_ = true;
                break;
            case 2:
                goal_control();
                enable_pass_control_ = true;
                break;
            case 3:
                goal_control();
                enable_pass_control_ = true;
                break;
            case 4:
                goal_control();
                enable_pass_control_ = true;
                break;
            case 5:
                goal_control();
                enable_pass_control_ = true;
                goal_thres = 0.3;
                break;
            case 6:
                wait();
                break;
        }
        last_status_ = current_status_;
    }
}

void DecisionCenter::pass_condition(const geometry_msgs::Point& goal,double thres=0.7)
{
    if(pow(current_pose_.position.x-goal.x,2)+pow(current_pose_.position.y-goal.y,2)<thres)
    {
        current_status_++;
        current_goal_++;
    }
}

void DecisionCenter::obstacle_detection_init()
{
    nh.getParam("obstacle_detect/min_angle",obstacle_params.min_angle);
    nh.getParam("obstacle_detect/max_angle",obstacle_params.max_angle);
    nh.getParam("obstacle_detect/start/x",obstacle_params.start.x);
    nh.getParam("obstacle_detect/start/y",obstacle_params.start.y);
    nh.getParam("obstacle_detect/end/x",obstacle_params.end.x);
    nh.getParam("obstacle_detect/end/y",obstacle_params.end.y);
}

void DecisionCenter::wait()
{
    nav_msgs::Path path;
    path.header.frame_id = "map";
    plan_pub_.publish(path);
}

void DecisionCenter::stop()
{
    std_msgs::Int8 limit_speed;
    limit_speed.data = 0;
    limit_speed_pub_.publish(limit_speed);
}

void DecisionCenter::limit_speed()
{
    std_msgs::Int8 limit_speed_msg;
    limit_speed_msg.data = 1;
    limit_speed_pub_.publish(limit_speed_msg);
}

void DecisionCenter::dislimit_speed()
{
std_msgs::Int8 dislimit_speed_msg;
dislimit_speed_msg.data = 2;
limit_speed_pub_.publish(dislimit_speed_msg);
}

void DecisionCenter::goal_control()    //负责向下发布当前的目标点
{
    nav_msgs::GetPlan srv;
    geometry_msgs::PoseStamped start;
    start.pose = current_pose_;
    start.header.frame_id = "map";
    geometry_msgs::PoseStamped goal;
    goal.pose.position = goal_list_[current_goal_];
    goal.pose.orientation.w = 1;
    goal.header.frame_id = "map";
    srv.request.start = start;
    srv.request.goal = goal;
    srv.request.tolerance = 0.5;
    if(plan_client_.call(srv))
    {
        plan_pub_.publish(srv.response.plan);
    }
    else
    {
        ROS_ERROR("Failed to call");
    }
}